#include <iostream>
#include <cmath>
#include "is_homography_matrix_R.h"

using namespace std;

IsMatrixR::IsMatrixR(void)
{

}

IsMatrixR::~IsMatrixR(void)
{

}
/*
 * This function will calculate the inverse matrix of rotation matrix
 * It works only with 3x3 mat given in an array of length 9.
 */
void IsMatrixR::calcInverseMat(double *PImgMatrix, int myMatLength)
{
    int RANK = 3;
    int LENGTH = myMatLength;
    kalmanType* a = new kalmanType[LENGTH];   
    
	// declare and construct the matrix.
    for(int i = 0; i < LENGTH; i++)
    {
        a[i] = *PImgMatrix;
        PImgMatrix++;
    }
    //calculate cofactors.
    kalmanType* matCofact = new kalmanType[LENGTH];

    matCofact[0] = (a[4]*a[8] - a[5]*a[7]);
    matCofact[1] = -1*(a[3]*a[8] - a[6]*a[5]);
    matCofact[2] = (a[3]*a[7] - a[6]*a[4]);

    matCofact[3] = -1*(a[1]*a[8] - a[7]*a[2]);
    matCofact[4] = (a[0]*a[8] - a[6]*a[2]);
    matCofact[5] = -1*(a[0]*a[7] - a[6]*a[1]);

    matCofact[6] = (a[1]*a[5] - a[4]*a[2]);
    matCofact[7] = -1*(a[0]*a[5] - a[3]*a[2]);
    matCofact[8] = (a[0]*a[4] - a[1]*a[3]);

    // calculate determinant
    double detA = a[0]*matCofact[0] + a[1]*matCofact[1] + a[2]*matCofact[2];

    // Inverse Mat : Transpose the matCofact and divide by determinant.
    for( int i = 0; i < RANK; i++)
    {
        for(int j = 0; j < RANK; j++)
        {
            IsMatrixR::imgInverseMatrix[j + i*RANK] = (matCofact[i + j*RANK])/detA;
        }
    }
#if 0
    // Tested ok -- with MATLAB.
    for(int j = 0; j < LENGTH; j++)
    {
        cout<<a[j]<<"\t";
    }
    cout<<endl;
    cout<<"\n--DEBUG: This is inverse matrix.\n";
    for(int j = 0; j < LENGTH; j++)
    {
        cout<<IsQuaternionR::imgInverseMatrix[j]<<"\t";
    }
#endif
    PImgMatrix = imgInverseMatrix;
    
#if 0
    cout<<"\n--DEBUG: This is inverse matrix.\n";
    for(int j = 0; j < LENGTH; j++)
    {
        cout<<*PImgMatrix<<"\t";
        PImgMatrix++;
    }
#endif
    delete[] a;
    delete[] matCofact;
  
}

/* NOTE : Tested OK with MATLAB.
 * ABOUT : This finction will multiply two matrix using cofactors.
 * This is written only for the matrix with rank 3.
 * Matrices should be in arrays of length 9.
 */
void IsMatrixR::multMatrix(kalmanType *pMyFirstMatrix, kalmanType *pimgSecondMatrix, int rank)
{
    const int LENGTH = rank*rank;
    kalmanType* a = new kalmanType[LENGTH];   
    kalmanType* b = new kalmanType[LENGTH];   
    
    // construct the matrix.
    for(int i = 0; i < LENGTH; i++)
    {
        a[i] = *pMyFirstMatrix;
        b[i] = *pimgSecondMatrix;
        pMyFirstMatrix++;
        pimgSecondMatrix++;
    }
    //calculate cofactors.
    for(int i = 0; i < rank; i++)
    {
        for(int j = 0; j < rank; j++)
        {
            IsMatrixR::imgMultMatrix[j+i*rank] = a[i*rank + 0] * b[j + 0*rank]
                                             + a[i*rank + 1] * b[j + 1*rank] 
                                             + a[i*rank + 2] * b[j + 2*rank];
        }
    }

#if 0
    printf("\n--DEBUG : Printing Matrix A..\n");
    for(int i = 0 ; i < rank*rank; i++)
    {
        cout<<a[i]<<"\t";
    }
    printf("\n--DEBUG : Printing Matrix B..\n");
    for(int i = 0 ; i < rank*rank; i++)
    {
        cout<<b[i]<<"\t";
    }
    printf("\n--DEBUG : Printing Matrix multMat..\n");
    for(int i = 0 ; i < rank*rank; i++)
    {
        cout<<IsQuaternionR::imgMultMatrix[i]<<"\t";
    }
#endif
    delete[] a;
    delete[] b;
   
}

